#!/usr/bin/env python

import roslib; roslib.load_manifest('DelphinROSv2')
import rospy
import smach
import smach_ros
import time

# Initialise class
# @extends: smach.State
#
# __init__ - initialises an Initialise object
# @param: lib - instance of library_highlevel.py
# @param: timeout - time state will wait for sensors/actuators to come online
# execute - waits until all sensors/actuators are online and checks motor voltage. 
# @return: aborted: if timeout is reached
# @return: preempted: if battery voltage is less than 20,000mV
# @return: succeeded: if all systems came online within the time and battery is above 20,000mV

class Initialise(smach.State):
	def __init__(self, lib, timeout):
		smach.State.__init__(self, outcomes=['succeeded','aborted','preempted'])
                self.__controller = lib
                self.__timeout = timeout
  		
	def execute(self,userdata):
		time_zero = time.time()
                all_online = False
                
                while (time.time()-time_zero < self.__timeout) and not(all_online):
                   all_online = (self.__controller.getTSLStatus() 
                        and self.__controller.getTailStatus()   
                        and self.__controller.getAltimeterStatus()
                        and self.__controller.getGPSStatus()
                        and self.__controller.getCompassStatus())
                   print 'TSL status = ', self.__controller.getTSLStatus()
                   print 'tail status = ', self.__controller.getTailStatus()
                   print 'alt status = ', self.__controller.getAltimeterStatus()
                   print 'gps status = ', self.__controller.getGPSStatus()
                   print 'compass status = ', self.__controller.getCompassStatus()
                   time.sleep(0.5)
                    
                #if timeout...
                print 'all online = ', all_online
                print 'time elapsed = ', time.time()-time_zero
                
                if all_online == False:     
                    rospy.logerr("One or more critical systems have not come online within the timeout (%ss)" %self.__timeout)               
                    return 'aborted'
                
                time.sleep(0.1) #give TSL board time to return a voltage measurement
                str = "Motor voltage: %smV" %(self.__controller.getVoltage())
                rospy.loginfo(str)
                
                voltage=self.__controller.getVoltage()
                if voltage < 20*1000:
                    rospy.logerr("Initial battery voltage, %smV < 20,000mV" %voltage)               
                    return 'preempted'
                else:
                    rospy.loginfo("All critical systems have come online within the timeout (%ss)" %self.__timeout) 
                    return 'succeeded'
                
                